clear
clc
format long g
clf

while 1
    sampled_data = csvread('~/ros/current/ws_acme/src/iiwa_trajectory_data/recorded_trajectory_2.csv',1,0);
    %assign columns into seperate variable names - this is made quickly using the
    %header names and find-replace command
    i=1;
    timestamp=sampled_data(:,i);i=i+1;
    t_delta=diff(timestamp);
    
    j1_pos_desired=sampled_data(:,i);i=i+1;
    j1_vel_desired=sampled_data(:,i);i=i+1;
    j1_pos=sampled_data(:,i);i=i+1;
    j1_vel=sampled_data(:,i);i=i+1;
    j1_vel_delta=diff(j1_vel);
    j1_accel=[0; j1_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    j2_pos_desired=sampled_data(:,i);i=i+1;
    j2_vel_desired=sampled_data(:,i);i=i+1;
    j2_pos=sampled_data(:,i);i=i+1;
    j2_vel=sampled_data(:,i);i=i+1;
    j2_vel_delta=diff(j2_vel);
    j2_accel=[0; j2_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    j3_pos_desired=sampled_data(:,i);i=i+1;
    j3_vel_desired=sampled_data(:,i);i=i+1;
    j3_pos=sampled_data(:,i);i=i+1;
    j3_vel=sampled_data(:,i);i=i+1;
    j3_vel_delta=diff(j3_vel);
    j3_accel=[0; j3_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    j4_pos_desired=sampled_data(:,i);i=i+1;
    j4_vel_desired=sampled_data(:,i);i=i+1;
    j4_pos=sampled_data(:,i);i=i+1;
    j4_vel=sampled_data(:,i);i=i+1;
    j4_vel_delta=diff(j4_vel);
    j4_accel=[0; j4_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    j5_pos_desired=sampled_data(:,i);i=i+1;
    j5_vel_desired=sampled_data(:,i);i=i+1;
    j5_pos=sampled_data(:,i);i=i+1;
    j5_vel=sampled_data(:,i);i=i+1;
    j5_vel_delta=diff(j5_vel);
    j5_accel=[0; j5_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    j6_pos_desired=sampled_data(:,i);i=i+1;
    j6_vel_desired=sampled_data(:,i);i=i+1;
    j6_pos=sampled_data(:,i);i=i+1;
    j6_vel=sampled_data(:,i);i=i+1;
    j6_vel_delta=diff(j6_vel);
    j6_accel=[0; j6_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    j7_pos_desired=sampled_data(:,i);i=i+1;
    j7_vel_desired=sampled_data(:,i);i=i+1;
    j7_pos=sampled_data(:,i);i=i+1;
    j7_vel=sampled_data(:,i);i=i+1;
    j7_vel_delta=diff(j7_vel);
    j7_accel=[0; j7_vel_delta ./ t_delta]; % add a zero in front to make same size as other data structures
    
    time_start_end = [timestamp(1,1) timestamp(end,1)];
    
    if (0)  %plot position actual
        plot(...
             timestamp, j1_pos,'r', ...
             timestamp, j2_pos,'y', ...
             timestamp, j3_pos,'m', ...
             timestamp, j4_pos,'c',...
             timestamp, j5_pos,'g',...
             timestamp, j6_pos,'b',...
             timestamp, j7_pos,'k')

        xlabel('Time')
        ylabel('Position')
        legend('Joint 1 Pos',...
               'Joint 2 Pos',...
               'Joint 3 Pos',...
               'Joint 4 Pos',...
               'Joint 5 Pos',...
               'Joint 6 Pos',...
               'Joint 7 Pos')

        title('Position Trajectory')
        
    elseif(0) %plot velocity actual
        plot(...
             timestamp, j1_vel,'r',...
             timestamp, j2_vel,'y',...
             timestamp, j3_vel,'m',...
             timestamp, j4_vel,'c',...
             timestamp, j5_vel,'g',...
             timestamp, j6_vel,'b',...
             timestamp, j7_vel,'k')

        xlabel('Time')
        ylabel('Velocity')
        legend('Joint 1 vel',...
               'Joint 2 vel',...
               'Joint 3 vel',...
               'Joint 4 vel',...
               'Joint 5 vel',...
               'Joint 6 vel',...
               'Joint 7 vel')

        title('Velocity Actual Trajectory')
    elseif(0) %plot velocity desired with actual
        plot(...
             timestamp, j1_vel_desired,'--r',...
             timestamp, j2_vel_desired,'--y',...
             timestamp, j3_vel_desired,'--m',...
             timestamp, j4_vel_desired,'--c',...
             timestamp, j5_vel_desired,'--g',...
             timestamp, j6_vel_desired,'--b',...
             timestamp, j7_vel_desired,'--k',...
             timestamp, j1_vel,'r',...
             timestamp, j2_vel,'y',...
             timestamp, j3_vel,'m',...
             timestamp, j4_vel,'c',...
             timestamp, j5_vel,'g',...
             timestamp, j6_vel,'b',...
             timestamp, j7_vel,'k')

        xlabel('Time')
        ylabel('Velocity')
        legend('Joint 1',...
               'Joint 2',...
               'Joint 3',...
               'Joint 4',...
               'Joint 5',...
               'Joint 6',...
               'Joint 7')

        title('Velocity DESIRED Trajectory')
    elseif(1) %plot acceleration desired with actual
        plot(...
             timestamp, j1_accel,'--r',...
             timestamp, j2_accel,'--y',...
             timestamp, j3_accel,'--m',...
             timestamp, j4_accel,'--c',...
             timestamp, j5_accel,'--g',...
             timestamp, j6_accel,'--b',...
             timestamp, j7_accel,'--k') %,...
%              timestamp, j1_vel,'r',...
%              timestamp, j2_vel,'y',...
%              timestamp, j3_vel,'m',...
%              timestamp, j4_vel,'c',...
%              timestamp, j5_vel,'g',...
%              timestamp, j6_vel,'b',...
%              timestamp, j7_vel,'k')

        xlabel('Time')
        ylabel('acceleration')
        legend('Joint 1',...
               'Joint 2',...
               'Joint 3',...
               'Joint 4',...
               'Joint 5',...
               'Joint 6',...
               'Joint 7')

        title('Acceleration')

    end
    
    %pause(1)
    break
end